#include "solve_astar.h"


namespace solve_astar_ns{
	int pd=0;
    struct star{
        int cost,expect,total,x,y;
        /*
        cost: evaluate cost of the point
        expect: evaluate xpectation of the point 
        toal:cost+expect
        x:row
        y:column
        */ 
    };
    bool operator <(star x,star y) // compare total first and if same, compare expect 
    {
        if (x.total!=y.total) return x.total>y.total;
        return x.expect>y.expect;
    }
    int wkx[4]={0,-1,0,1};
    int wky[4]={-1,0,1,0};
    Point father[1000][1000];
	star d[1000+2][1000+2];
};

using namespace solve_astar_ns;
using std::cin;
using std::cout;
using std::endl;
using std::priority_queue;
    
void solve_astar(Point start, Point end) // end means the end point
{
	
    priority_queue<star> q; //take out the minimun point of the total value from priority queue
    bool vis[row+2][col+2];
    memset(vis,false,sizeof(vis));
    father[start.x][start.y]=start;	
	
	for(int i=0;i<row;i++) // init expect and total
	{
        for(int j=0;j<col;j++)
        {
            d[i][j].x=i;
            d[i][j].y=j;
            d[i][j].total=d[i][j].expect=abs(i-end.x)+abs(j-end.y); //Manhattan distance
            d[i][j].cost=10000;//init to a big value
        }

	}

	d[start.x][start.y].cost=0;
	q.push(d[start.x][start.y]);//init the startpoint and push into prio-queue
	while (!q.empty())
	{
		star u_star=q.top();
		Point u;
		u.x=u_star.x;
        u.y=u_star.y;
		q.pop();		
		
		vis[u.x][u.y]=true;
		for (int a=0;a<4;a++)
		{
			Point v;
			v.x=u.x+wkx[a];
			v.y=u.y+wky[a];

			if (!map[v.x][v.y]&&!vis[v.x][v.y]&&d[v.x][v.y].cost>(d[u.x][u.y].cost+1)) // if good to go: not visit and not block 
			{
				astarnum++;
				d[v.x][v.y].cost=d[u.x][u.y].cost+1;
				d[v.x][v.y].total=d[v.x][v.y].cost+d[v.x][v.y].expect;
                //if (v.x==end.x&&v.y==end.y) return;
                father[v.x][v.y]=u;
				q.push(d[v.x][v.y]);
				vis[v.x][v.y]=true;
                Set_Grapics(v.y*2,v.x,9);cout<<"*";
				if (v.x==end.x&&v.y==end.y){
					pd=1;return;
				}
			}
		}
	}


	
}



void print_astar_path(int x, int y)
{
	Stack path_x,path_y;	
    path_x.push(x);
    path_y.push(y);
	while (true)
	{
		int father_x = father[x][y].x;
		int father_y = father[x][y].y;
		if (father_x == x && father_y == y) 
		{
			break;
		}
		else 
		{
			path_x.push(father_x); 
            path_y.push(father_y); 
			x=father_x;
			y=father_y;
		}
	}
	while (path_x.Ssize())
	{
		Set_Grapics(path_y.pop()*2,path_x.pop(),12);cout<<"*";
	}
}





void print_astar_ans(int x, int y)
{
    cout<<endl<<endl;
	if(!pd)
    {
        cout<<"No Answer!"<<endl;
        return;
	}
	Stack path_x,path_y;	
    path_x.push(x);
    path_y.push(y);
	while (true)
	{
		int father_x = father[x][y].x;
		int father_y = father[x][y].y;
		if (father_x == x && father_y == y) 
		{
			break;
		}
		else 
		{
			path_x.push(father_x); 
            path_y.push(father_y); 
			x=father_x;
			y=father_y;
		}
	}
	int num=1;
	while (path_x.Ssize()) 
	{
		cout<<"("<<path_x.pop()<<","<<path_y.pop()<<","<<num<<")"<<endl;
		num++;
	}
}


